#include "ATCSimulator.h"
#include "search_algorithms.h"
#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <algorithm>
#include <map>

ATCSimulator::ATCSimulator(int numAirplanes, int numAirports, int delayLeewayInput, int timeLimitInput)
{
    distMatrix = new double*[numAirports];
    for(int i = 0; i < numAirports; i++)
        distMatrix[i] = new double[numAirports];

/*
    // initialization
    double mapsize = 100.0;     // Makes a 100unitx100unit map to bound location values of airports
    double maxdistance = 4*mapsize;  // Makes the airport 'unconnected' if it is farther away than 30units
    vector<pair<double, double> > locAirports;
    for (int i=0; i<numAirports; i++)
    {
        double xloc = mapsize*((double)rand()/(double)RAND_MAX);
        double yloc = mapsize*((double)rand()/(double)RAND_MAX);
        locAirports.push_back(make_pair(xloc, yloc));
    }

    for (int i=0; i<numAirports; i++)
    {
        for (int j=0; j<numAirports; j++)
        {
            double xi = locAirports[i].first, yi = locAirports[i].second;
            double xj = locAirports[j].first, yj = locAirports[j].second;
            distMatrix[i][j] = pow(pow(xi-xj, 2.0)+pow(yi-yj, 2.0), 0.5)+1.0;
            if (distMatrix[i][j]>maxdistance) distMatrix[i][j]=INFINITY;
        }
    }

*/


    // initialization
    distMatrix[0][0] = 0;
    distMatrix[0][1] = 10;
    distMatrix[0][2] = 10;
    distMatrix[0][3] = 10;
    distMatrix[1][0] = 10;
    distMatrix[1][1] = 0;
    distMatrix[1][2] = 10;
    distMatrix[1][3] = 10;
    distMatrix[2][0] = 10;
    distMatrix[2][1] = 10;
    distMatrix[2][2] = 0;
    distMatrix[2][3] = 10;
    distMatrix[3][0] = 10;
    distMatrix[3][1] = 10;
    distMatrix[3][2] = 10;
    distMatrix[3][3] = 0;

    delayLeeway = delayLeewayInput;
    timeLimit = timeLimitInput;
    curTime = 0;

    //Create Lookup Table for paths.
    for(int i = 0; i < numAirports; i++)
    {
        vector<vector<vector<int> > > temp = DijkstraK(distMatrix, 4, i, 2);
        lookupTable.push_back(temp);
    }
    for(int i = 0; i < numAirports; i++)
    {
        Airport *a = new Airport();
        a->airportNum = i;
        a->planesInAirspace = 0;
        a->planesInAirspaceMax = 5;
        airports.push_back(*a);
    }

    for(int i = 0; i < numAirports; i++)
    {
        vector<Edge> tempVec;
        for(int j = 0; j < numAirports; j++)
        {
            Edge *e = new Edge;
            e->edgeNum = i;
            e->length = distMatrix[i][j];
            e->fromAirport = &airports[i];
            e->toAirport = &airports[j];
            tempVec.push_back(*e);
        }
        edges.push_back(tempVec);
    }


    for(int i = 0; i < numAirplanes; i++)
    {
        createAirplane();
    }
    for(int i = 0; i < numAirports; i++)
    {
        for(int j = 0; j < numAirports; j++)
        {
            sort(edges[i][j].airplaneDepatureQueue.begin(), edges[i][j].airplaneDepatureQueue.end(), less_than_key());
        }
    }
}

void ATCSimulator::startSimulation()
{
    curTime = 0;
    while(!simulatorDone())
    {
        update();
    }
}

void ATCSimulator::resetSimulation(int numPlanes)
{
    //Clear edges[airportDepart][airportArrival].airplaneDepatureQueue
    for(unsigned int i =0; i < airports.size(); i++)
    {
        for(unsigned int j = 0; j < airports.size(); j++)
        {
            edges[i][j].airplaneDepatureQueue.clear();
        }
    }
    airplanes.clear();
    //ReCreate Airplanes
    for(int i = 0; i < numPlanes; i++)
    {
        createAirplane();
    }
    for(unsigned int i = 0; i < airports.size(); i++)
    {
        for(unsigned int j = 0; j < airports.size(); j++)
        {
            sort(edges[i][j].airplaneDepatureQueue.begin(), edges[i][j].airplaneDepatureQueue.end(), less_than_key());
        }
    }
    for(unsigned int i = 0; i < airports.size(); i++)
    {
        airports[i].planesInAirspace = 0;
    }
}
void ATCSimulator::update()
{
    //if(rand()/RAND_MAX >= 0.9) createAirplane();
    for(unsigned int i = 0; i < airplanes.size(); i++)
    {
        //If the airplane 'should' be in the air, update its position. If the plane is not in the air due to congestion, the plane
        //will not take off. If if the MIT condidtion is met, it will take off.
        if(curTime >= airplanes[i].departureTime && airplanes[i].state != LANDED)
        {

            Edge *curEdge = &edges[airplanes[i].currentAirport][airplanes[i].edgeTravel[0]];

            airplanes[i].updatePosition(*curEdge);

            //If the airplane location == the length of the edge it should land.
            if(airplanes[i].edgeLoc >= edges[airplanes[i].currentAirport][airplanes[i].edgeTravel[0]].length)
            {
                //Find the edge the airplane was on and delete it from that edge.
                int it = 0;
                for(unsigned int j = 0; j < curEdge->airplanes.size(); j++)
                {
                    if(curEdge->airplanes[j]->airplaneNum == airplanes[j].airplaneNum)
                    {
                        it = j;
                    }
                }
                curEdge->airplanes.erase(curEdge->airplanes.begin() + it);

                //If this is the final destination...
                if(airplanes[i].arrivalAirport == airports[airplanes[i].edgeTravel[0]].airportNum)
                {
                    //Make the airplane LANDED, update the current airport.
                    airplanes[i].state = LANDED;
                    airplanes[i].currentAirport = airports[airplanes[i].edgeTravel[0]].airportNum;
                    airports[airplanes[i].edgeTravel[0]].airplanesLanded.push_back(airplanes[i].airplaneNum);
                    airplanes[i].justLanded = true;
                }
                //If this is just a hop...
                else
                {
                    //Make the plane in standby and add it to the
                    airplanes[i].state = STANDBY;
                    airplanes[i].currentAirport = airports[airplanes[i].edgeTravel[0]].airportNum;
                    airplanes[i].edgeTravel.erase(airplanes[i].edgeTravel.begin());
                    airplanes[i].edgeLoc = 0;
                }
            }
        }
    }

    for(unsigned int i = 0; i < airports.size(); i++)
    {
        airports[i].updateCongestion(edges);
    }
    curTime++;
}

bool ATCSimulator::simulatorDone()
{
    if(curTime == timeLimit) return true;
    return false;

}

void ATCSimulator::updateEdges(Airplane plane)
{
    for(unsigned int i = 0; i < edges.size(); i++)
    {
        for(unsigned int j = 0; j < edges[i].size(); j++)
        {
            for(unsigned int k = 0; k < edges[i][j].airplaneDepatureQueue.size(); k++)
            {
                if(edges[i][j].airplaneDepatureQueue[i].airplaneNum == plane.airplaneNum)
                {
                    edges[i][j].airplaneDepatureQueue.erase(edges[i][j].airplaneDepatureQueue.begin() + k);
                }
            }
        }
    }
    if(!plane.edgeTravel.empty()) edges[plane.currentAirport][plane.edgeTravel[0]].airplaneDepatureQueue.push_back(plane.airplaneNum);
}
double ATCSimulator::getReward()
{
    double total = 0;
    double totalDelay = 0;
    double totalTime = 0;
    for(unsigned int i = 0; i < airplanes.size(); i++)
    {
        //If the plane has just landed, count it in the throughput.
        if(airplanes[i].state == LANDED && airplanes[i].justLanded)
        {
            total += 1;
            if(airplanes[i].totalTime - (airplanes[i].arrivalTime - airplanes[i].departureTime) > 0)
            {
                totalDelay+= airplanes[i].totalTime - (airplanes[i].arrivalTime - airplanes[i].departureTime);
            }
            totalTime += airplanes[i].totalTime;
            airplanes[i].justLanded = false;
        }
    }
    //Trashed this idea for scaling reward. Wasn't negative enough.
    double lateModifier;
    if(totalTime == 0 && totalDelay == 0)
    {
        lateModifier = 1;
    }
    else
    {
        lateModifier = (totalTime/(totalTime + totalDelay) - 0.5) * 2.0;
    }
    //Just subtract the total delay from throughput.
    return (total/(double)1.0) - totalDelay;
}

void ATCSimulator::createAirplane()
{


    int airportDepart = rand() % (airports.size() - 1);
    int airportArrival;
    do
    {
        airportArrival = rand() % (airports.size() - 1);
    }
    while(airportArrival == airportDepart);

    //Debug. Every combination of 6+ airplanes. If timeLimit > 10 then all six should land. And six
    //additional should land every timestep.
/*
    int airportDepart;
    int airportArrival;
    if(airplanes.size() % 6 == 0){airportArrival = 0;airportDepart = 1;}
    if(airplanes.size() % 6 == 1){airportArrival = 0;airportDepart = 2;}
    if(airplanes.size() % 6 == 2){airportArrival = 1;airportDepart = 0;}
    if(airplanes.size() % 6 == 3){airportArrival = 1;airportDepart = 2;}
    if(airplanes.size() % 6 == 4){airportArrival = 2;airportDepart = 0;}
    if(airplanes.size() % 6 == 5){airportArrival = 2;airportDepart = 1;}
*/
    Airplane *a = new Airplane(airplanes.size());

    a->state = STANDBY;
    a->arrivalAirport = airportArrival;
    a->departureAirport = airportDepart;
    a->currentAirport = airportDepart;
    a->milesInTow = 0;
    a->justLanded = false;
    a->edgeLoc = 0;
    a->edgeTravel = lookupTable[airportDepart][airportArrival][0];
    a->prevState = 0;
    a->oneStop = true;
    a->totalTime = 0;
    //a->departureTime = curTime + (rand() % timeLimit);
    a->departureTime = 0;

    int totalTime = 0;
    int fromAirport = airportDepart;
    int toAirport;

    //Calculate estimated arrival time if they took the best possible route.
    for(unsigned int i = 0; i < lookupTable[airportDepart][airportArrival][0].size(); i++)
    {
        if(i == 0)
        {
            fromAirport = a->currentAirport;
            toAirport = lookupTable[airportDepart][airportArrival][0][0];
        }
        else
        {
            fromAirport = lookupTable[airportDepart][airportArrival][0][i-1];
            toAirport = lookupTable[airportDepart][airportArrival][0][i];
        }
        totalTime += distMatrix[fromAirport][toAirport];
    }
    a->arrivalTime = a->departureTime + totalTime + delayLeeway;

    airplanes.push_back(*a);
}

State* ATCSimulator::getState(Airplane plane)
{
    State* s = new State;
    int numRoutes = lookupTable[plane.currentAirport][plane.arrivalAirport].size();
    s->congestionFirst = 0;
    s->congestionSecond = 0;
    s->congestionThird = 0;
    //Congestion at first,second,third route
    for(int i = 0; i < numRoutes; i++)
    {
        if(i == 0) s->congestionFirst = airports[lookupTable[plane.currentAirport][plane.arrivalAirport][0][0]].planesInAirspace;
        if(i == 1) s->congestionSecond = airports[lookupTable[plane.currentAirport][plane.arrivalAirport][1][0]].planesInAirspace;
        if(i == 2) s->congestionThird = airports[lookupTable[plane.currentAirport][plane.arrivalAirport][2][0]].planesInAirspace;
    }

    //Planes routed to same first, second or third route.
    int totalFirst = 0;
    int totalSecond = 0;
    int totalThird = 0;
    for(unsigned int i = 0; i < airplanes.size(); i++)
    {
        if(airplanes[i].edgeTravel.empty()) continue;
        if(airplanes[i].edgeTravel[0] == lookupTable[plane.currentAirport][plane.arrivalAirport][0][0]) totalFirst++;
        if(numRoutes > 1)
        {
            if(airplanes[i].edgeTravel[0] == lookupTable[plane.currentAirport][plane.arrivalAirport][1][0]) totalSecond++;
        }
        if(numRoutes > 2)
        {
            if(airplanes[i].edgeTravel[0] == lookupTable[plane.currentAirport][plane.arrivalAirport][2][0]) totalThird++;
        }
    }
    s->routedFirst = totalFirst;
    s->routedSecond = totalSecond;
    s->routedThird = totalThird;

    s->speed = plane.speed;
    s->timeLeft = (plane.arrivalTime - plane.departureTime) - plane.totalTime;

    for(unsigned int i = 0; i < states.size(); i++)
    {
        if(*states[i] == *s) return states[i];
    }
    states.push_back(s);
    return s;
}


void ATCSimulator::updateAgents(vector<double> rewardList, vector<pair<State*,Actions> > stateActionPairs, int t, int rewardStep)
{
    //Update agents that do not have a next state. Do this by using the reward in the future.
    for(unsigned int i = 0; i < stateActionPairs.size(); i++)
    {
        State* state = stateActionPairs[i].first;
        int action = stateActionPairs[i].second;
        double reward = 0;
        for(int j = t; j < t+rewardStep; j++)
        {
            reward += rewardList[j];
        }
        reward = reward/rewardStep;
        double nextReward = 0;
        for(int j = t+rewardStep; j < t+rewardStep*2; j++)
        {
            nextReward+= rewardList[j];
        }
        nextReward = nextReward/rewardStep;
        for(unsigned int j = 0; j < airports.size(); j++)
        {
            airports[j].agent->updateReward(state, action, reward, nextReward);
        }
    }
}

void ATCSimulator::updateAgents(vector<double> rewardList, vector<dataStruct> stateActionPairs, int t, int rewardStep)
{
    //Update agents that have a next state.
    for(unsigned int i = 0; i < stateActionPairs.size(); i++)
    {
        State* prevState = stateActionPairs[i].prevState;
        int prevAction = stateActionPairs[i].prevAction;
        State* state = stateActionPairs[i].state;
        double reward = 0;
        for(int j = t; j < t+rewardStep; j++)
        {
            reward += rewardList[j];
        }
        reward = reward/rewardStep;
        double nextReward = 0;
        for(int j = t+rewardStep; j < t+rewardStep*2; j++)
        {
            nextReward+= rewardList[j];
        }
        nextReward = nextReward/rewardStep;
        for(unsigned int j = 0; j < airports.size(); j++)
        {
            airports[j].agent->updateReward(prevState, prevAction, state, reward);
        }
    }
}
ATCSimulator::~ATCSimulator()
{
    //dtor
}
